#include <stdio.h>
#include "state.h"

//------------------------------------------------------------------------------
// input file format - (t, d_theta_x, d_theta_y, d_theta_z, d_v_x, d_v_y, d_v_z)
// inputs measured in the body frame
int main() {
	STATE s;
	FILE *i = fopen("input.txt", "r");
	FILE *f = fopen("log.txt",   "w");
	double pt=0, t, dt;
	double dtheta[3];
	double dv[3];
	
	_init();
	_state_init(&s, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
	_state_log_init(f);
	_state_log(f, pt, &s);
	while(!feof(i)) {
		fscanf(i, "%lf %lf %lf %lf %lf %lf %lf\n", &t, &dtheta[0], &dtheta[1], &dtheta[2], &dv[0], &dv[1], &dv[2]);
		dt = t - pt;
		_state_update(&s, dtheta, dv, dt);
		_state_log(f, t, &s);
		pt = t;
	}
	_state_destroy(&s);
	_destroy();
	
	fclose(i);
	fclose(f);
	return 0;
}
